#include <Arduino.h>
#include <Servo.h> 
#include <avr/io.h>
#include <avr/interrupt.h>
#include "config.h"
#include "motors.h"
#include "ADC.h"
#include "realese.h"
#include "Timer.h"

Servo wheelServoR;


// this function check value it return the value in diapazon from -30 to +30  
int checkServo( int value)
{
  value = value>(SERVO_MAX_ANGLE)?SERVO_MAX_ANGLE:value;
  value = value<(-SERVO_MAX_ANGLE)?-SERVO_MAX_ANGLE:value;
  return value;
}
// depends of global class wheelServo 
// this function write value to servo and wait time in miliseconds
// this function use checkServo so the value could be any integer. 
int writeServoW (int value , int waitTime)
{
  if (wheelServoR.attached() == false)
  {
    wheelServoR.attach(SERVO_PIN);
  }
  value = checkServo(value);
  wheelServoR.write(STCALC(value));
  delay(waitTime);
  wheelServoR.detach();
  return value;
}
// depends of global class wheelServo 
// this function write value to servo without waiting time
// this function use checkServo so the value could be any integer. 
int writeServo (int value )
{
  if (wheelServoR.attached() == false)
  {
    wheelServoR.attach(SERVO_PIN);
  }
  value = checkServo(value);
  wheelServoR.write(STCALC(value));
  return value;
}

// return value in sm it used for convert from right analod sharp sensor to sm
double read_gp2d12_range(double value)
{
  double val;
  
  val = 8044.0721/(value - 129.0100)-0.9581;
  if (val < 0) val = 80;
  if (val >80) val = 80;
  //val = (8762.88/(value/4 + 18.32)-12.06)/2;
  return val; 
}
// return value in sm it used for convert from center analod sharp sensor to sm
double read_gp2y0a_range(double value)
{
  double val;
  val = (8762.88/(value/4 + 18.32)-12.06);
  if (val < 0) val = 150;
  if (val >150) val = 150;
  
  return val; 
} 
// detect 
int haveObstacle(int ledPinInfra, int InputPin)
{
  static const int wait_time = 28;
  static int val = HIGH;
  static int number = 0;
  static const int ledPinRed = 13;
  val = HIGH;
  number = 0;
  for(int i=0;i<50;i++)
  {
    //digitalWrite(ledPinInfra, LOW);
    digitalWrite(ledPinInfra, HIGH);
    delayMicroseconds(wait_time);
    digitalWrite(ledPinInfra, LOW);

    delayMicroseconds(wait_time);
    val = digitalRead(InputPin);
    if (val == HIGH) {
      number++;
      digitalWrite(ledPinRed, LOW);
    } 
    else {
      digitalWrite(ledPinRed, HIGH);
    }
  }
  return number;
}

void robotInit()
{
  analogReference(INTERNAL2V56); //analog init  
  adcInit();
  TimerThreeInit();


  motor_init();  // init pins and set motor to stop

  //button init (set pul-up resistor)
  pinMode(BUTTON, INPUT);
  digitalWrite(BUTTON,HIGH);
  
  // contact sensors init // this is button
  pinMode(CONTACT_SENSOR_LEFT, INPUT);
  digitalWrite(CONTACT_SENSOR_LEFT,HIGH);
  pinMode(CONTACT_SENSOR_RIGHT, INPUT);
  digitalWrite(CONTACT_SENSOR_RIGHT,HIGH);
   pinMode(CONTACT_SENSOR_CENTER, INPUT);
  digitalWrite(CONTACT_SENSOR_CENTER,HIGH);

  // init tsop sensors
  pinMode(SENSOR_DIGITAL_LEFT,INPUT);
  digitalWrite(SENSOR_DIGITAL_LEFT,HIGH);
  pinMode(SENSOR_LED_LEFT,OUTPUT);
  
  pinMode(SENSOR_DIGITAL_RIGHT,INPUT);
  digitalWrite(SENSOR_DIGITAL_RIGHT,HIGH);
  pinMode(SENSOR_LED_RIGHT,OUTPUT);
  
  pinMode(SENSOR_DIGITAL_CENTER,INPUT);
  digitalWrite(SENSOR_DIGITAL_CENTER,HIGH);
  pinMode(SENSOR_LED_CENTER,OUTPUT);
    
  // initialize the LED pin as an output:
  pinMode(LED,OUTPUT);     

  // init servo and set it to midle
  wheelServoR.attach(SERVO_PIN);  
  wheelServoR.write(STCALC(0));

  Serial1.begin(57600); // start UART
  Serial.begin(115200);
  
  // say START
  delay(1000);  
  Serial1.print('S');
  Serial.print('S');
}
